#pragma once

#include <fastdds/dds/domain/DomainParticipant.hpp>
#include <fastdds/dds/publisher/DataWriter.hpp>
#include <fastdds/dds/publisher/DataWriterListener.hpp>
#include <fastdds/dds/publisher/Publisher.hpp>
#include <fastdds/dds/topic/TypeSupport.hpp>
#include <thread>
#include <fastdds/dds/domain/DomainParticipantFactory.hpp>
#include <fastdds/dds/publisher/qos/DataWriterQos.hpp>
#include <fastdds/dds/publisher/qos/PublisherQos.hpp>

template <typename msg_type>
class DDSPublisher {
public:
  // Constrcutor declared in header file since it is a temlate func
  
  DDSPublisher(msg_type, std::string topic_name,
               eprosima::fastdds::dds::DomainParticipant *participant)
      : publisher_(nullptr), topic_(nullptr), writer_(nullptr),
        type_(new msg_type) {

    // Set topic name
    topic_name_ = topic_name;

    // Set pointer to domain participant
    participant_ = participant;

    init();
  }

  DDSPublisher():publisher_(nullptr), topic_(nullptr), writer_(nullptr),
        type_(new msg_type){}

  
  bool init(std::string topic_name,
               eprosima::fastdds::dds::DomainParticipant *participant){

                    // Set topic name
                    topic_name_ = topic_name;

                    // Set pointer to domain participant
                    participant_ = participant;

                    return init();
               }


  // Topic Name
  std::string topic_name_{};

  virtual ~DDSPublisher(){
      if (writer_ != nullptr) {
    publisher_->delete_datawriter(writer_);
  }
  if (publisher_ != nullptr) {
    participant_->delete_publisher(publisher_);
  }
  if (topic_ != nullptr) {
    participant_->delete_topic(topic_);
  }

  }

  bool init(){
  // REGISTER THE TYPE
  type_.register_type(participant_);

  // CREATE THE PUBLISHER
  publisher_ = participant_->create_publisher( eprosima::fastdds::dds::PUBLISHER_QOS_DEFAULT, nullptr);
  if (publisher_ == nullptr) {
    return false;
  }

  // CREATE THE TOPIC
  topic_ = participant_->create_topic(topic_name_, type_.get_type_name(),
                                       eprosima::fastdds::dds::TOPIC_QOS_DEFAULT);
  if (topic_ == nullptr) {
    return false;
  }

  // CREATE THE WRITER
  writer_ =
      publisher_->create_datawriter(topic_,  eprosima::fastdds::dds::DATAWRITER_QOS_DEFAULT, &listener);
  if (writer_ == nullptr) {
    return false;
  }

  std::cout << "Position DataWriter created." << std::endl;
  return true;

  // Set Best effort QOS for datawriter
  best_effort_.kind =  eprosima::fastdds::dds::BEST_EFFORT_RELIABILITY_QOS;
  //best_effort_.kind = RELIABLE_RELIABILITY_QOS;
  writer_qos_.reliability(best_effort_);
  writer_qos_.publish_mode().kind = eprosima::fastdds::dds::SYNCHRONOUS_PUBLISH_MODE;
  //writer_qos_.resource_limits().max_samples = 1000;
  //writer_qos_.resource_limits().allocated_samples = 500;
	
  writer_->set_qos(writer_qos_);

  };

  template <typename T> void publish(T msg) {
    while (listener.matched() == 0) {
      std::this_thread::sleep_for(
          std::chrono::milliseconds(1)); // Sleep 1 ms
    }

    // Publish Message
    writer_->write(&msg);
  }

    template <typename T> void publish_wait(T msg ,int64_t ms) {
    if(listener.matched() == 0)
    {
            std::this_thread::sleep_for(
          std::chrono::milliseconds(ms)); // Sleep 1 ms
    }

    if(listener.matched()  !=  0)
    {
      // Publish Message
      writer_->write(&msg);
    }
  }

private:
  eprosima::fastdds::dds::DomainParticipant *participant_;
  eprosima::fastdds::dds::Publisher *publisher_;
  eprosima::fastdds::dds::Topic *topic_;
  eprosima::fastdds::dds::DataWriter *writer_;
  eprosima::fastdds::dds::TypeSupport type_;

  eprosima::fastdds::dds::ReliabilityQosPolicy best_effort_;
  eprosima::fastdds::dds::DataWriterQos writer_qos_;

public:
  class PubListener : public eprosima::fastdds::dds::DataWriterListener {

  public:
    PubListener() = default;
    ~PubListener() override = default;

  private:
    void on_publication_matched(
        eprosima::fastdds::dds::DataWriter *writer,
        const eprosima::fastdds::dds::PublicationMatchedStatus &info) {
            if (info.current_count_change == 1) {
    matched_ = info.total_count;
    std::cout << "DataWriter matched." << std::endl;
  } else if (info.current_count_change == -1) {
    matched_ = info.total_count;
    std::cout << "DataWriter unmatched." << std::endl;
  } else {
    std::cout << info.current_count_change
              << " is not a valid value for PublicationMatchedStatus current "
                 "count change"
              << std::endl;
  }


        }

    int matched_ = 0;

  public:
    /// Getter function
    const float matched() const { return matched_; }

  } listener;
};

//#include"default_publisher.cpp"
